Planning of the motion of a multi-motor control system is generally performed in two steps: path planning and motion planning. Collision-free trajectory planning is typically solved by decomposing the problem into two steps. First, only the geometric aspects of the multi-motor control system, including the collision avoidance constraint, are considered to produce a collision-free path for the multi-motor control system to follow. Next, the planning, which ends up with a trajectory, accounts for other constraints, such as the constraints on system dynamics to specify how the multi-motor control system moves along the collision-free path. After the trajectory is determined, a speed profile can be assigned to the trajectory to determine the movement of a mass of the multi-motor control system. Such decomposition can be computationally efficient. However, that approach can fail to find a feasible trajectory when the geometric path is not properly generated. Also, this approach does not ensure an optimal trajectory.
The planning of an optimal trajectory of a multi-motor control system avoiding obstacles while minimizing energy consumption of the multi-motor control system is difficult. This is because the requirement of avoiding obstacles usually leads to a set of non-convex constraints and the optimization problem involving a continuous-time dynamical system and non-convex constraints is a difficult problem to solve.
Several conventional methods optimize energy efficiency during the path planning step by associating geometry, e.g., curvature of the trajectory with energy consumption, thus no dynamic constraints are considered. Some methods consider the energy-efficient path planning problem in terms of the friction and gravity, but assume the mass in the multi-motor control system moves at a constant speed. Results on the energy optimal motion planning are limited, and some methods focus on time-optimality and smoothness of the trajectory, see, e.g., U.S. Pat. No. 6,216,058.
The non-convex constraints due to obstacle avoidance need to be enforced during trajectory generation and optimization. Several conventional methods formulate the trajectory planning with obstacle avoidance as an optimization problem. For example, the method described in U.S. Pat. No. 7,248,952 formulates the trajectory planning as a mixed integer linear programming problem. However, the computation of a mixed integer problem is time consuming. Another method, described by Hagenaars et al, in “Approximate continuous-time optimal control in obstacle avoidance by time/space discretization of non-convex state constraints,” Proc. of the 2004 IEEE Int. Conf. on Control Applications pp. 878-883, 2004, considers optimal control with obstacles by discretization of time and space. However, the cost function of the state and control variables is convex.
Some methods determine a suboptimal trajectory planning by minimizing influence of the non-convex constraints. For example, a method described by Duleha et al., in “Nonholonomic motion planning based on newton algorithm with energy optimization,” IEEE Transactions on Control System Technology, Vol: 11(3), pp. 355-363, May 2003, considers energy efficient trajectory planning for non-holonomic systems without obstacles. Yet another method described by Mei et al., in “Energy-efficient motion planning for mobile robots,” In Proc. of IEEE Int. Conf. on Robotics and Automation, pp. 4344-4349, 2004, considers the energy efficient motion planning for a mobile robot which has to visit several locations. The robot is assumed to move at certain pattern, thus simplifying the energy optimal trajectory planning problem by ignoring the dynamics of robots. However, all those methods do not take obstacles into account.
Accordingly, there is a need for optimization of a trajectory of a motion of a multi-motor control system avoiding at least one obstacle, wherein the optimization is subject to spatial non-convex constraints due to the obstacle.